package TrafficSim;

import java.awt.Point;
import java.util.Hashtable;
import java.util.Iterator;
import java.util.NoSuchElementException;
import java.util.Set;
import java.util.Vector;

import javax.vecmath.Vector2d;

public class Intersection {

	private Point position;
	// private int id; //For debugging only
	private Hashtable<Intersection, Street> Streets;
	private Vector<Street> allStreets;
	private Vector<Street> allStreetsByClock;
	private Vector<Street> incomingStreetsByClock;
	private Vector<Street> outgoingStreetsByClock;
	TrafficLightController tlc;
	private int id;
	public enum TrafficDirection {RIGHT, LEFT, STRAIGHT, REVERSE};

	public int getId(){
		return id;
	}
	
	
	public Intersection(int id, int x, int y) {
		this.position = new Point(x, y);
		this.id = id;
		// this.id= id; //For debugging only
		this.Streets = new Hashtable<Intersection, Street>();
		this.allStreets = new Vector<Street>();
	}

	public Set<Intersection> getNextIntersections() {
		return this.Streets.keySet();
	}

	public Street getStreet(Intersection i) {
		return this.Streets.get(i);
		
	}

	/**
	 * Adds a street connecting this Intersection with another one to the
	 * hashtable. Also shifts the Street approximately 2 pixels right parallel
	 * to the actual street (for display purpose)
	 * 
	 * @param i
	 *            the "other" Intersection
	 * @param s
	 *            the Street connecting the Intersections (One Way)
	 */
	public void appendStreet(Intersection i, Street s) {
		Vector2d v = new Vector2d(this.position.getX(), this.position.getY());
		Vector2d x = new Vector2d(i.position.getX(), i.getPosition().getY());
		Vector2d temp = new Vector2d(i.position.getX(), i.getPosition().getY());
		x.sub(v); // get the Position vector of the Street
		Vector2d orth = new Vector2d(-x.y, x.x); // create a vector orthogonal
													// to it
		orth.normalize(); // set to length 1
		orth.scale(3); // for better viewing
		// orth.absolute();
//		System.err.println("I AM NOT SUPPOSED TO DO THIS!!!!");
		v.add(orth); // shift the lane approximately 2 pixels
		temp.add(orth); // orthogonal to the actual lane
		Point p = new Point((int) v.x, (int) v.y);
		Point po = new Point((int) temp.x, (int) temp.y);
		s.initStreet(p, po); // "Build" Street
		s.calcDirection();
		this.Streets.put(i, s); // add connection to hashtable
		addStreetClockwise(s);
		i.addStreetClockwise(s);
	}
	public String toString() {
		return "Intersection ID: "+ id;
	}
	public void addTrafficLightController(){
		tlc = new TrafficLightController(this);
		tlc.turnOn();
		tlc.toggleOn();
	}
	public TrafficLightController getTLC(){
		return this.tlc;
	}

	public Point getPosition() {
		return this.position;
	}

	public void addStreetClockwise(Street s) {
		allStreets.add(s);
	}

	public Street getNextStreetAntiClockWise(Street st) {

		Iterator<Street> it = this.allStreetsByClock.iterator();
		Street s = it.next();
		while (it.hasNext()) {
			if (s == st) {
				try {
					return it.next();
				} catch (NoSuchElementException ex) {
					return allStreetsByClock.firstElement();
				}
			}
			s = it.next();
		}
		return allStreetsByClock.firstElement();
	}
	
	public Street getNextOutStreetAntiClockWise(Street st) {
		try {
			Iterator<Street> it = this.allStreetsByClock.iterator();
			Street s = it.next();
			Street tempS = it.next();
			while (it.hasNext()) {
				if (s == st) {
					if (outgoingStreetsByClock.contains(tempS)) {
						return tempS;
					}

					else {
						return getNextOutStreetAntiClockWise(tempS);
					}
				}
				s = tempS;
				tempS = it.next();
			}
			it = this.allStreetsByClock.iterator();
			s = it.next();
			tempS = it.next();
			while (it.hasNext()) {
				if (s == st) {
					if (outgoingStreetsByClock.contains(tempS)) {
						return tempS;
					}

					else {
						return getNextOutStreetAntiClockWise(tempS);
					}
				}
				s = tempS;
				tempS = it.next();
			}
			return st;
		} catch (NoSuchElementException ex) {
			return st;
		}
	}

	public Street getPrevStreetAntiClockWise(Street st) {
		try {
			Iterator<Street> it = this.allStreetsByClock.iterator();
			Street tempS = incomingStreetsByClock.lastElement();
			Street s = it.next();
			while (it.hasNext()) {
				if (s == st) {
					if (incomingStreetsByClock.contains(tempS)) {
						return tempS;
					}

					else {
						return getPrevStreetAntiClockWise(tempS);
					}
				}
				tempS = s;
				s = it.next();
			}
			return st;
		} catch (NoSuchElementException ex) {
			return st;
		}
	}
	public Vector<Street> getIncomingStreets(){
		return this.incomingStreetsByClock;
	}

	private Street truncateStreet(Street s) {
		Vector2d dir = new Vector2d(s.getEnd().x, s.getEnd().y);
		dir.sub(new Vector2d(s.getStart().x, s.getStart().y));
		dir.normalize();
		dir.scale(20);
		Street temp = new Street(1);
		Vector2d start = new Vector2d(s.getStart().x, s.getStart().y);
		Vector2d end = new Vector2d(s.getEnd().x, s.getEnd().y);
		start.add(dir);
		end.sub(dir);
		temp.initStreet(new Point((int) start.x, (int) start.y), new Point(
				(int) end.x, (int) end.y));
		temp.calcDirection();
		return temp;
	}

	private Point getStartPoint(Street s) {
		Point start = s.getStart();
		if (s.getStart().distance(this.position) > s.getEnd().distance(
				this.position)) {
			start = s.getEnd();
		}
		return start;
	}

	public void sort() {
		Street lowest = getLowest();
		allStreetsByClock = new Vector<Street>();
		incomingStreetsByClock = new Vector<Street>();
		outgoingStreetsByClock = new Vector<Street>();
		if (!(Streets.values().contains(lowest))) {
			incomingStreetsByClock.add(lowest);
		}
		allStreetsByClock.add(lowest);
		allStreets.remove(lowest);
		Street tempLow = truncateStreet(lowest);
		Point lo = getStartPoint(tempLow);
		while (allStreets.size() > 0) {
			double furthestRight = 360;
			Street nextStreet = allStreets.firstElement();
			for (Street s : this.allStreets) {
				Street thisStreet = truncateStreet(s);
				Point thisStart = getStartPoint(thisStreet);
				double angle = getAngleBetweenVectors(position, lo, thisStart);
				if (angle < furthestRight) {
					nextStreet = s;
					furthestRight = angle;
				}
				if (angle == furthestRight) {
					if (lo.distance(thisStart) < lo
							.distance(getStartPoint(nextStreet))) {
						nextStreet = s;
						furthestRight = angle;
					}
				}
			}
			allStreetsByClock.add(nextStreet);
			allStreets.remove(nextStreet);
			if (!(Streets.values().contains(nextStreet))) {
				incomingStreetsByClock.add(nextStreet);
			}
			else{
				outgoingStreetsByClock.add(nextStreet);
			}
		}
	}
	public int getOrderOfNextStreet(Street s, Street next){
		Street st = getNextOutStreetAntiClockWise(s);
		int i = 1;
		while (st != next){
			st = getNextOutStreetAntiClockWise(st);
			i++;
		}
		return i;
	}
	public TrafficDirection getRealDirection(Street s, Street next){
		Vector2d orth = new Vector2d(0,-1);
		double angle = s.getDirection().angle(next.getDirection())*180/Math.PI;
		System.out.println(angle);
		if (angle > 160){
			return TrafficDirection.STRAIGHT;
		}
		if (angle < 20){
			return TrafficDirection.REVERSE;
		}
		return null;
	}
	public TrafficDirection getDirection(Street s, Street next){
		Street a = truncateStreet(s);
		Street b = truncateStreet(next);
		Vector2d t1 = new Vector2d(this.position.x, this.position.y);
		t1.sub(new Vector2d(a.getStart().x, a.getStart().y));
		Vector2d t2 = new Vector2d(this.position.x, this.position.y);
		t2.sub(new Vector2d(b.getStart().x, b.getStart().y));
		double calcedAngle = t1.angle(t2)*180/Math.PI;
		double angle = getAngleBetweenVectors(this.position, a.getEnd(), b.getStart());
		if (0 < calcedAngle && calcedAngle < 160){
			if (270 < angle && angle <= 360){
				return TrafficDirection.RIGHT;
			}
			if (0 <= angle && angle <= 270){
				return TrafficDirection.LEFT;
			}
		}
		else{
			Vector2d dir = new Vector2d(next.getEnd().x, next.getEnd().y);
			dir.sub(new Vector2d(next.getStart().x, next.getStart().y));
			dir.negate();
			Vector2d dir2 = new Vector2d(s.getEnd().x, s.getEnd().y);
			dir2.sub(new Vector2d(s.getStart().x, s.getStart().y));
			if(dir2.getX() == dir.getX() && dir2.getY() == dir.getY()){
				return TrafficDirection.REVERSE;
			}
			return TrafficDirection.STRAIGHT;
		}
		System.out.println("wtf");
		return null;
	}
	public Street getRightOncomingStreet(Street actual){
		Street s = getNextStreetAntiClockWise(actual);
		for (int i = 0; i< this.allStreets.size(); i++){
			Street temp = getNextStreetAntiClockWise(s);
			if (incomingStreetsByClock.contains(temp)){
				if(getDirection(actual, temp) == TrafficDirection.RIGHT){
					return temp;
				}
			}
		}
		return null;		// no Street RIGHT
	}
	public Street getStraightOncomingStreet(Street actual){
		Street temp = getNextStreetAntiClockWise(actual);
		for (int i = 0; i< this.allStreetsByClock.size(); i++){
			if (incomingStreetsByClock.contains(temp)){
				if(getDirection(actual, temp) == TrafficDirection.STRAIGHT){
					return temp;
				}
			}

			temp = getNextStreetAntiClockWise(temp);
		}
		return null;		// no Street STRAIGHT ahead
	}

	public Street getStraightOncomingStreetForLeftDriving(Street actual){

		int index = this.allStreetsByClock.indexOf(actual);
		Street s = this.allStreetsByClock.get((index+2)%this.allStreetsByClock.size());
		return s;
	}
	private Street getLowest() {
		Street lowest = this.allStreets.firstElement();
		Point min = getStartPoint(lowest);
		for (Street s : this.allStreets) {
			Street temp = truncateStreet(s);
			Point p = getStartPoint(temp);
			if (p.y > min.y) {
				min = p;
				lowest = s;
			}
			if (p.y == min.y) {
				if (p.x > min.x) {
					min = p;
					lowest = s;
				}
			}
		}
		return lowest;
	}

	private double getAngleBetweenVectors(Point origin, Point lowest,
			Point streetStart) {
		Vector2d t1 = new Vector2d(origin.x, origin.y);
		t1.sub(new Vector2d(lowest.x, lowest.y));
		Vector2d t2 = new Vector2d(origin.x, origin.y);
		t2.sub(new Vector2d(streetStart.x, streetStart.y));
		double result;

		double opp;
		double adj;
		// calculate vector differences
		opp = t1.y - t2.y;
		adj = t1.x - t2.x;
		result = (Math.atan(opp / adj)) * 180 / Math.PI;
		if(adj == 0.0){
			result = (Math.atan2(adj,opp))*180/Math.PI;
		}

		if(result>0.0){
			result=180+(180-result);
			}
		else
		{
			result=180-(180-Math.abs(result));
			}
		return result;
	}
}
